#pragma once

#include <vector>
#include <mutex>
#include <deque>
#include <memory>
#include <iostream>
#include <iomanip>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

static int print_precision = 4;

namespace cartographer
{
    struct TimedPose
    {
        common::Time time;
        transform::Rigid3d pose;
    };

    template<typename Type>
    inline bool HasNan(const Eigen::Matrix<Type, 3, 1> &data)
    {
        return std::isnan(data(0)) || std::isnan(data(1)) || std::isnan(data(2));
    }

    template<typename Type>
    std::string VecToStr(const Eigen::Matrix<Type, 3, 1> &data)
    {
        std::stringstream sss;
        sss << data(0) << "," << data(1) << "," << data(2);
        return sss.str();
    }

    inline void Print(const char *name, const transform::Rigid3d &tf)
    {
        std::stringstream sss;
        sss << std::setprecision(print_precision) << name << " tr: "
            << tf.translation().x() << ","
            << tf.translation().y() << ","
            << tf.translation().z() << " rt: "
            << tf.rotation().w() << ","
            << tf.rotation().x() << ","
            << tf.rotation().y() << ","
            << tf.rotation().z();
        LOG(INFO) << sss.str();
    }

    template<typename T>
    inline void Print(const char *name, const Eigen::Vector<T, 3> &data)
    {
        std::stringstream sss;
        sss << std::setprecision(print_precision) << name << data(0) << "," << data(1) << "," << data(2) << "\n";
        LOG(INFO) << sss.str();
    }
} // namespace cartographer